package TrafficSim;


import java.awt.Point;
import java.io.File;
import java.io.FileWriter;
import java.io.IOException;
import java.util.Iterator;
import java.util.NoSuchElementException;
import java.util.Vector;

import javax.vecmath.*;

public class  Vehicle {

	
	public enum TrafficDirection {RIGHT, LEFT, STRAIGHT};
	
	private Vector<Intersection> route = new Vector<Intersection>();
	private Iterator<Intersection> iterator;
	private Intersection currentIntersection = null;
	private Intersection nextIntersection = null;
	private float acceleretion;
	private double deceleration;
	private Vector2d currentSpeedVec;
	private double currentSpeed = 0;
	private static File file;
	private Vector2d direction;
	private Vector2d posOld = new Vector2d(0, 0);
	private Vector2d posNew = new Vector2d(0, 0);
	private boolean nearIntersection = false;
	double brakingDistance;
	private boolean distanceCarInFrontOk = true;
	private float curveSpeed = 20.0f;
	private float maxSpeed;
	private Street lastStreet = null;
	private Street currentStreet = null;
	private float t = 1.0f;
	private double distanceToNextIntersection;
	private float deltaT = 0.1f;
	private static int vehicleID = 0;
	private int ID = 0;
	private VehicleStatus vehicleStatus;
	public final int usageConst = 9;
	public final int usageAcc = 19;
	public final int usageDec = 7;
	public final int usageWait = 7;
	private int usageSum = 0;
	private int steps = 0;
	private int stepsWaiting = 0;
	private double totalDistance = 0.0;
	private int nextIntID = 1;
	private int stoppingTime = 0;
	private boolean turnLeft = false;
	

	public Vehicle(Vector2d position, int i) { // i wird nur einfach so mit
												// �bergeben, sonst zwei gleiche
												// construktoren!!!
		this.setID(); // ID wird angelegt, muss und darf nur im constructor
						// gemacht werden
		this.posNew = position;
	}

	public Vehicle(float acc, float maxSpeed, float dec,
			Vector<Intersection> routeNav) { // wird sp�ter standard Konstruktor
		this.setID(); // ID wird angelegt, muss und darf nur im constructor
						// gemacht werden
		this.acceleretion = acc;
		this.maxSpeed = maxSpeed;
		this.deceleration = dec;
		this.route = routeNav;
		iterator = route.iterator();
		this.getCurrentIntersection();
		this.nextIntersection = iterator.next();
		this.vehicleStatus = VehicleStatus.ACCELERATION;
		this.currentStreet = this.currentIntersection.getStreet(route.get(1));
		this.direction = this.currentStreet.getDirection();
		this.currentSpeedVec = new Vector2d(0, 0);
		posNew = new Vector2d(this.currentStreet.getStart().getX(),
				this.currentStreet.getStart().getY());
		this.currentStreet.enterringStreet(this);
		this.totalDistance += this.calcStreetLength();
		if (Master.greenWave){
			Vehicle.file = new File("output_gw_on.txt");
		}
		else{
			Vehicle.file = new File("output_gw_off.txt");
		}
	}
	
	private double calcStreetLength() {
		Vector2d lengthStreetEnd = new Vector2d();
		try{
		Vector2d lengthStreet = new Vector2d(this.currentStreet.getStart().getX(), this.currentStreet.getStart().getY());
		lengthStreetEnd = new Vector2d(this.currentStreet.getEnd().getX(), this.currentStreet.getEnd().getY());
		lengthStreetEnd.sub(lengthStreet);		
		}catch(NullPointerException ex){
			calcUsage();
		}
		double len = lengthStreetEnd.length();

		return len;
	}

	public Vector2d calcPosConstSpeed() { // berechnet position wenn v konstant
		Vector2d puffer = new Vector2d(this.posOld);
		Vector2d v1 = new Vector2d(this.currentSpeedVec);
		v1.scale(this.t);
		puffer.add(v1);
		this.t += this.deltaT;
		return puffer;

	}

	public Vector2d testcalcPosDecSpeed() { // berechnet pos wenn beschl. Bewegung
		Vector2d puffer = new Vector2d(this.posNew);
        
        Vector2d d = new Vector2d(direction);
        d.normalize();
        
        double speed = this.currentSpeedVec.length();
//        double speed2 = speed - (this.calcDeceleration()*deltaT);       
        double speed2 = speed - (deceleration*deltaT);  
        this.currentSpeedVec = new Vector2d(d);
        this.currentSpeedVec.scale(speed2);
        
        speed = Math.pow(speed, 2);
        speed2 = Math.pow(speed2, 2);
        
        double weg = (speed - speed2) / (2*this.deceleration);

//        System.out.println("weg verzoegern " + weg);
        d.scale(weg);
        puffer.add(d);
        return puffer;

	}


	public Vector2d testcalcPosConstSpeed() {
		Vector2d puffer = new Vector2d(this.posNew);
		double speed = this.currentSpeedVec.length();
		speed *= deltaT;	
		Vector2d d2 = new Vector2d(this.direction.getX(), this.direction.getY());
		d2.normalize();
		d2.scale(speed);
		puffer.add(d2);
		return puffer;
	}
	
	public int getCurrentUsage(){
		switch (this.vehicleStatus){
		case ACCELERATION: 
			return this.usageAcc;
		
		case CONSTANT:
			if (this.currentSpeed < 0.2){
				return this.usageWait;
			}
			return this.usageConst;
		
		case DECELERATION:
			return this.usageDec;
		}
		return 1;
	}
	
	public Vector2d testcalcPosAccSpeed() { // berechnet pos wenn beschl. Bewegung
        Vector2d puffer = new Vector2d(this.posNew);
        Vector2d d = new Vector2d(direction);
        d.normalize();
        double speed = this.currentSpeedVec.length();
        double speed2 = speed + (this.acceleretion*deltaT);
        Vector2d vec1 = new Vector2d(d);
        vec1.scale(speed2);
        this.currentSpeedVec.set(vec1);
        speed = Math.pow(speed, 2);
        speed2 = Math.pow(speed2, 2);
        speed = (speed2 - speed) / (2*this.acceleretion);
//        System.out.println("weg in methode: " + speed);
        vec1.set(d);
        vec1.scale(speed);
        vec1.add(puffer);
        return vec1;

	}
	

	public Vector2d getSpeed() { // gibt die momentan Geschwindigkeit wieder als
									// Vector
		return this.currentSpeedVec;
	}

	public double calcSpeed() { // gibt momentan Geschwindigkeit wieder als
								// Fliesskommazahl wieder
		return this.currentSpeedVec.length();
	}


	public void switchPos() { // einfacher Switch der Positionen
		this.posOld.set(this.posNew);
	}

	public void tReset(float f) { // resettet die Zeit
		this.t = f;
	}

	public boolean streetChangeNecessary(){
		if (this.distanceToNextIntersection <= 1.5 && this.distanceToNextIntersection >= 0.0) {
			this.currentStreet.leavingStreet(this);
			if (this.streetChange()){
				this.currentStreet.enterringStreet(this);
				this.nearIntersection = false;
				this.direction = this.currentStreet.getDirection();
				double speedVecLength = this.currentSpeedVec.length();
				this.currentSpeedVec.set(this.direction.getX(), this.direction
					.getY());
				this.currentSpeedVec.normalize();
				this.currentSpeedVec.scale(speedVecLength);

				this.posNew.set(this.currentStreet.getStart().getX(),
					this.currentStreet.getStart().getY());
//				this.switchPos();
//				this.tReset(deltaT);
			}
			else {
				return false;
			}		
			
		}
		return true;
	}

	
	public boolean update() {
		
		if (this.currentSpeed >= 1 && this.curveSpeed != 0.0f) // kontrollstruktur zur Berechnung des Durchschnittsverbrauchs
		{
		this.usageSum += this.getCurrentUsage();
		this.steps++;
		}
		
		if (this.currentSpeed <= 0.1 || this.curveSpeed == 0.0f)
		{
//			this.usageWaitingSum += this.usageWait;
			this.stepsWaiting++;
		}
		
		this.distanceCarInFrontOk = this.calcDistanceToCarInFront();
		this.nearIntersection = this.checkIfNearIntersection();
		if (this.nearIntersection){
			this.switchCurveSpeed();
		}
		boolean status = this.streetChangeNecessary();
		if (!status){
			this.calcUsage();
			return false;
		}
		controlStatus();
		return true;

	}
	
	public static void writeFiles(String s){

		try {
			FileWriter fw = new FileWriter(file, true);
//			fw.write(s + "\n");
			fw.append(s + "\n");
//			System.out.println("geschrieben");
			fw.close();
		} catch (IOException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		}
		
//		System.out.println(s);
	}
	
	public void calcUsage(){

		float v = ((float)this.stepsWaiting / (float)(this.stepsWaiting + this.steps)) * this.usageWait;
		float average = ((float)(this.usageSum / this.steps)+v);
		String s = "FahrzeugID: " + "\n" + this.ID + "Zur�ckgelegter Weg: " + this.totalDistance + "\n" + "Gesamtzeit: " +
		(this.steps + this.stepsWaiting)*0.1/60 + "\n" + "Wartezeit: " + this.stepsWaiting*0.1/60 + "\n" +
		"Durchschnittsverbrauch " + average + "\n" + "CO2: " + ((average * this.totalDistance/1000.0 * 2.33) / 100.0) + " KG!"
		+ "\n" + " Standzeit: " + this.stoppingTime*this.deltaT + "\n Fahrzeit: " + this.deltaT * this.steps;
		Vehicle.writeFiles(s);
	}

	public boolean checkContraflow(){
		Street oncomingStreet = nextIntersection.getStraightOncomingStreet(this.currentStreet);
//		Street oncomingStreet = this.nextIntersection.getStraightOncomingStreetForLeftDriving(this.currentStreet);
		if (oncomingStreet == null){
//			System.out.println("null");
			return false;
		}
		if (oncomingStreet.getVehicles().size() == 0)
		{
//			System.out.println("size 0");
			return false;
		}
		Vehicle vehicle = null;
		try{
		vehicle = oncomingStreet.getVehicles().firstElement();
		}catch(NoSuchElementException ex){
			return false;
		}
		if (vehicle == null){
			return false;
		}
		double distance = vehicle.getDistanceToNextIntersection();
		double timeToCross = Math.sqrt(40 / this.acceleretion);
//		timeToCross = 10;
		double timeSecondCar = distance / vehicle.currentSpeed;
		if (timeToCross < timeSecondCar){
//			System.out.println("durch");
			return false;
		}
		else {
//			System.out.println("durch");
			return true;
		}
		
	}
	
	public void logStoppingTime()
	{
		TrafficLight.State t;
		t = this.currentStreet.getTrafficLight().getTlState();
		if ((t == TrafficLight.State.RED || t == TrafficLight.State.REDYELLOW || t == TrafficLight.State.YELLOW) && this.currentSpeed < 1) 
		{
			this.stoppingTime++;
		}
	}
	
	private void switchCurveSpeed() {
		TrafficLight.State t;
		if (!this.currentStreet.existingTL()){
			t = TrafficLight.State.GREEN;
		}
		else {
			t = this.currentStreet.getTrafficLight().getTlState();
			this.logStoppingTime();
		}
		Street nextStreet = getNextStreet();
		Intersection.TrafficDirection td = Intersection.TrafficDirection.STRAIGHT;
		if (nextStreet != null){
			td = this.nextIntersection.getDirection(currentStreet,getNextStreet());
		}
		switch(t){
		
		case GREEN:
			switch(td){
			case RIGHT:
				
				this.curveSpeed = 25.0f;
				break;
			case LEFT:
				this.turnLeft = true;
				if (this.checkContraflow())
					this.curveSpeed = 0.0f;
				else
					this.curveSpeed = 25.0f; // test auf gegenverkehr n�tig!
				break;
			case STRAIGHT:
				this.curveSpeed = this.currentStreet.getSpeedLimit();
				break;
			case REVERSE:
				this.curveSpeed = 25.0f;
				break;
				
			}
			break;
		
		case RED:
			if (!(this.distanceToNextIntersection <= 20)){
				this.curveSpeed = 0.0f;
			}
			break;
		
		case REDYELLOW:
			this.curveSpeed = 0.0f;
			break;
		
		case YELLOW:
			if (!(this.distanceToNextIntersection <= 20)){
				this.curveSpeed = 0.0f;
			}
			break;
		
		}
		
	}
	public Vector<Intersection> getRoute(){
		return this.route;
	}

	private boolean calcDistanceToCarInFront() {
			Vehicle v = this.currentStreet.getNextVehicle(this);
			if (v != null)
			{
				if (v.getTurnLeft()){
					return true;
				}
			Vector2d posOfCarInFront = new Vector2d(v.getPosition());
			posOfCarInFront.sub(this.posNew);
			double distanceToCarInFront = posOfCarInFront.length();
			return (distanceToCarInFront > (this.currentSpeed*3.6/2)+8);
			}

			else {
				if (this.nearIntersection){
					Street nextStreet = this.getNextStreet();
					if (nextStreet == null){
						return true;
					}

					if (nextStreet.getVehicles().size() == 0){
						return true;
					}
					Vehicle lastVehicleInStreet;
					try {
						lastVehicleInStreet = nextStreet.getVehicles().get(nextStreet.getVehicles().size()-1);
					}
					catch (Exception e){
						return true;
					}
					Point p = nextStreet.getStart();
					Vector2d p1 = lastVehicleInStreet.getPosition();
					Vector2d p2 = new Vector2d(p.getX(), p.getY());
					p2.sub(p1);
					double length = p2.length();
					
					
					if (length <= 20){	
						return false;
					}
				}
				
				
				
				return true;
			}
				
				

	}

	private boolean checkIfNearIntersection() {
		this.distanceToNextIntersection = this.calcDistance();
		this.brakingDistance = ((this.currentSpeed * this.currentSpeed)/(2 * this.deceleration))+20;
		return this.distanceToNextIntersection <= this.brakingDistance;
	}

	/**
	 * @return the direction
	 */
	public Vector2d getDirection() {
		return direction;
	}

	private void controlStatus() {

		switch (this.vehicleStatus){
		
		case ACCELERATION:
			this.posNew =this.testcalcPosAccSpeed();
			this.currentSpeed = calcSpeed();
			
			// Pr�fen, ob die Geschwindigkeitsbegrenzung der Stra�e oder des Autos erreicht ist
			if (this.currentSpeed >= (this.currentStreet.getSpeedLimit()/3.6) || this.currentSpeed >= (this.maxSpeed/3.6)){

				this.vehicleStatus = VehicleStatus.CONSTANT;

				if (this.currentSpeed >= this.currentStreet.getSpeedLimit()/3.6) {
					this.currentSpeed = this.currentStreet.getSpeedLimit()/3.6;
					this.currentSpeedVec.set(this.direction);
					this.currentSpeedVec.normalize();
					this.currentSpeedVec.scale(this.currentSpeed);
				}
				else {
					this.currentSpeed = this.maxSpeed/3.6;
					this.currentSpeedVec.set(this.direction);
					this.currentSpeedVec.normalize();
					this.currentSpeedVec.scale(this.currentSpeed);
				}
			}
			// Pr�fen, ob der Abstand zum anderen Auto eingehalten wird
			if (!this.distanceCarInFrontOk)	{
				this.vehicleStatus = VehicleStatus.DECELERATION;
				}
			if (this.nearIntersection) {
				this.vehicleStatus = VehicleStatus.DECELERATION;
				}
			break;
			
		case CONSTANT:
			this.posNew = this.testcalcPosConstSpeed();
			
			// Pr�fen, ob die Geschwindigkeit von Auto und Stra�e unterschritten wird und nicht wegen der Kreuzung gebremst wird und auch der Abstand zum Auto ok ist
			if (this.currentSpeed < this.currentStreet.getSpeedLimit()/3.6 && this.currentSpeed < (this.maxSpeed/3.6) &&
					!this.nearIntersection && this.distanceCarInFrontOk) {				
				this.vehicleStatus = VehicleStatus.ACCELERATION;
			}
			// Pr�fen, ob Mindestabstand nicht ok oder Bremspunkt erreicht und Geschwindigkeit h�her als Kurvengeschwindigkeit
			if (!this.distanceCarInFrontOk || (this.nearIntersection && this.currentSpeed > this.curveSpeed/3.6 )){
				this.vehicleStatus = VehicleStatus.DECELERATION;

			}
			break;
		
		case DECELERATION:
			this.posNew =this.testcalcPosDecSpeed();
			this.currentSpeed = calcSpeed();

			if (this.currentSpeed <= (this.currentStreet.getSpeedLimit()/3.6) && !this.nearIntersection && this.distanceCarInFrontOk) {
				this.vehicleStatus = VehicleStatus.CONSTANT;
				this.currentSpeedVec.set(this.direction);
				this.currentSpeedVec.normalize();
				this.currentSpeedVec.scale(this.currentSpeed);	
			}			
			if (this.nearIntersection && this.currentSpeed <= this.curveSpeed/3.6 && this.distanceCarInFrontOk){
				this.vehicleStatus = VehicleStatus.CONSTANT;
				this.currentSpeed = this.curveSpeed/3.6;
				this.currentSpeedVec.set(this.direction);
				this.currentSpeedVec.normalize();
				this.currentSpeedVec.scale(this.curveSpeed/3.6);
			}
			break;		
		}
	}


	private double calcDistance() {
		Vector2d p1 = new Vector2d(this.getPosition());
		Vector2d p2 = new Vector2d(this.currentStreet.getEnd().getX(),
				this.currentStreet.getEnd().getY());
		p2.sub(p1);
		return p2.length();
	}


	public boolean streetChange() {
		
		if (iterator.hasNext()) {
//			System.out.println(this.currentIntersection.getId());
			this.getNextIntersection();
			this.lastStreet = currentStreet;
			this.currentStreet = this.currentIntersection
			.getStreet(this.nextIntersection);
			this.totalDistance += this.calcStreetLength();
			this.turnLeft = false;
			return true;
		}
		return false;

	}

	public void getCurrentIntersection() {
		if (iterator.hasNext()) {
			this.currentIntersection = iterator.next();
		}
	}

	public void getNextIntersection() {

			this.switchIntersection();
			this.nextIntID  ++;
			this.nextIntersection = iterator.next();
	}

	public Street getCurrentStreet(){
		return this.currentStreet;
	}
	public Street getlastStreet(){
		return this.lastStreet;
	}
	public Street getNextStreet(){
		Street nextStreet = null;
		try{
//			System.out.println(currentIntersection +" "+ nextIntersection);
		nextStreet = nextIntersection.getStreet(this.route.get(nextIntID+1));
		}catch (ArrayIndexOutOfBoundsException ex){
			return null;
		}
		if (nearIntersection){
//			try{
////				System.out.println(currentIntersection +" "+ nextIntersection);
//			nextStreet = nextIntersection.getStreet(this.route.get(route.indexOf(nextIntersection)+1));
//			}catch (ArrayIndexOutOfBoundsException ex){
//				return null;
//			}
		}
		return nextStreet;
	}
	
	public Intersection returnNextIntersection(){
		return nextIntersection;
	}
	public Intersection returnCurrentIntersection(){
		return this.currentIntersection;
	}
	public void switchIntersection() {
		this.currentIntersection = this.nextIntersection;
	}

	public void updateVehicle(float time) {

	}
	public boolean isNearIntersection(){
		return this.nearIntersection;
	}

	public Vector2d getPosition() {
		return this.posNew;
	}

	public static int getVehicleID() {
		return vehicleID;
	}

	public static synchronized void setVehicleID() {
		vehicleID++;
	}

	public int getID() {
		return this.ID;
	}

	private void setID() {
		setVehicleID();
		this.ID = getVehicleID();
	}

	public double getDistanceToNextIntersection() {
		return distanceToNextIntersection;
	}
	
	public boolean getTurnLeft(){
		return this.turnLeft;
	}
	
}